/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Filters;

import Jama.Matrix;
/**
 *
 * @author Anne
 */
public class Kalman2 {
    
    private double[] source;
    private Matrix sourceM;
    private Matrix signals;
    private double kalmanGain;  // scalar matrix
    private double optBayEstimate;
    private double variance; // scalar matrix
    private double measurementECG;
    private double pNC; // processNoiseCovariance scalar matrix
    private double mNC; //measurementNoiseCovariance scalar matrix
    
    private int length;
    private int N = 5; // number of heartbeats  1-25
    
    private double tempX, tempV, tempK;
    
    public Kalman2(){
        
    }
    /**
     * Constructor in case of no control.
     * @param A is an array of 3 ECG signals, the source is the fourth one
     */
    public void model(double[][] A, double[] source){
        this.source = source;
        length = source.length;
        sourceM = new Matrix(source, length);
        //optBayEstimate = ;
        //variance = ;
        //mNC ;  // section 3D
        
        signals = new Matrix(A);
        
        for(int k = 0; k < length; k++){
            
            calculateMNC(k); // calculate mNC equation 3/4
            
            calculatePNC(k); // calculate process Noise(pNC)  equation  14 
            
            updateKalmanGain(); //equation 9
            
            update(k);
            
            // equation 11
        }
    }
    
    public void update(int k){
        measurementECG = source[k];
        
        tempX = optBayEstimate + kalmanGain * (measurementECG - optBayEstimate);
        tempV = variance + pNC - kalmanGain * (variance + pNC);
        
        optBayEstimate = tempX;
        variance = tempV;
    }
    
    public void updateKalmanGain(){
        tempK = (variance + pNC) / (mNC + variance + pNC);
        kalmanGain = tempK;
    }
    
    public void calculateMNC(int k){
        Matrix MSE = new Matrix(length, 1);
        Matrix temp = (signals.times(signals.transpose())).inverse();
        Matrix temp2 = sourceM.times(signals.transpose());
        MSE = temp.times(temp2);

        Matrix output = signals.times(MSE);
        mNC = source[k] - output.get(0, 0);
    }
    
    public void calculatePNC(int k){
        
    }
    
    public double getMNC(){
        return mNC;
    }
    
    public double getPNC(){
        return pNC;
    }
    
    public double getKalmanGain(){
        return kalmanGain;
    }
    
    public double getOptBayEstimate(){
        return optBayEstimate;
    }
    
    public double getVariance(){
        return variance;
    }
    
    public double getMeasurementECG(){
        return measurementECG;
    }
}
